package com.rockwell.robot2015.test;

import java.util.ArrayList;
import java.util.Arrays;

import com.rockwell.robot2015.model.GameStage;
import com.rockwell.robot2015.model.IRobotLogger;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.RobotConfig;
import com.rockwell.robot2015.model.states.RobotStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningState;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;
import com.rockwell.robot2015.model.states.running.sequence.FindingDropAreaSequence;
import junit.framework.TestCase;

public class FindingDropAreaSequenceTester extends TestCase {
	
	Robot m_robot;
	RobotStateMachine m_mainMachine;
	RobotRunningStateMachine m_runningMachine;
	FindingDropAreaSequence m_sequence;
	
	class TestData {
		boolean[] dropAreaPresent;
		boolean[] leftSensor;
		boolean[] rightSensor;
		boolean[] frontSensor;
		double[][] positionData;
	}
	ArrayList<String> traceMessage = new ArrayList<String>();
	DummyRobotCommander commander = new DummyRobotCommander();
	private IRobotLogger m_logger = new IRobotLogger() {

		@Override
		public void log(final String msg) {
			traceMessage.add(msg);
		}

		@Override
		public void log(final Exception ex) {
			StackTraceElement[] trace = ex.getStackTrace();
			traceMessage.add(trace.toString());
		}

		@Override
		public void setCurrentState(String s) {
			
		}
		
		@Override
		public void onRobotPaused() {
		}

		@Override
		public void onRobotResumed() {
		}

		@Override
		public void onRobotIdle() {
		}
    };
	
    private void prepare(TestData data) {
		
		DummyObstacleDetector dropAreaDetector = new DummyObstacleDetector(data.dropAreaPresent, data.dropAreaPresent);
		DummyLRFRobotSensors sensor=new DummyLRFRobotSensors(null,null,data.leftSensor,data.rightSensor,data.frontSensor);
		RobotConfig config = new RobotConfig();
		config.width = 15;
		config.length = 15;
		config.sensors = sensor;
		config.commander = commander;
		config.predefinedMazeSolver = null;
		config.treasureDetector = null;
		config.obstacleDetector = null;
		config.maze1Detector = null;
		config.outerWallDetector = null;
		config.dropAreaDetector = dropAreaDetector;
		
		m_robot = new Robot(config, GameStage.Finals);

		m_robot.setLogger(m_logger, 4);
		m_robot.setStepMode(false);
		
		traceMessage.clear();
		
		m_mainMachine = m_robot.stateMachine();
		m_runningMachine = m_mainMachine.runningStateMachine();
	}
    
    @Override
	public void setUp() {
	}
    
    public void testRunSeq() {
	    TestData data = new TestData()
		{{
			dropAreaPresent = new boolean[] { 
					false, false, false, false, false, false, false, false, false, false,
					false, false, false, false, false, false, false, false, false, false,
					false, false, false, false, false, false, false, false, false, false,
					false, false, false, false, true,
			};
			leftSensor = new boolean[] { 
					true, false, true, false, true, false, true, true, true, false,
					true, false, true, false, true, true, true, true, true, false,
					true, false, true, true, true, true, true, true, true, true,
					true, true , true, true, true,
					false,
			};
			rightSensor = new boolean[] { 
					true, false, true, true, true, true, true, true, true, false,
					true, true, true, true, true, false, true, true, true, true,
					true, false, true, true, true, false, true, false, true, false,
					true, false, true, false, true,
					false,
					
			};
			frontSensor = new boolean[] { 
					false, true, false, true, false, false, false, true, false,	true,
					false, true, false, false, false, true, false, true, false, true,
					false, true, false, true, false, false, false, true, false, false,
					false, true, false, false, false,
					false,
					
			};
			positionData = new double[][] { 
					{0, 1, 1}, //test case 1
					{0, 1, 1}, //test case 2
					{0, 1, 1}, //test case 1
					{0, 1, 1}, //test case 2
					{0, 1, 1}, //test case 1
					{0, 1, 1}, //test case 2
					{0, 1, 1}, //test case 1
					{0, 1, 1}, //test case 2
					{0, 1, 1}, //test case 1
					{0, 1, 1}, //test case 2
					{0, 1, 1}, //test case 1
					{0, 1, 1}, //test case 2
					
			};
		}};
		
		//ArrayList<RobotAction> expectedActions = new ArrayList<RobotAction>();
		ArrayList<RobotAction> expectedActions = new ArrayList<RobotAction>(
		        Arrays.asList(		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		RobotAction.Jog,
		        		RobotAction.TurnBack,
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		RobotAction.Jog,
		        		RobotAction.TurnRight,
		        		RobotAction.Jog,
		        		RobotAction.TurnBack,
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		
		        		RobotAction.Jog,
		        		RobotAction.TurnLeft,
		        		RobotAction.Jog,
		        		RobotAction.TurnBack,
		        		RobotAction.Jog,
		        		RobotAction.Jog,
		        		RobotAction.TurnRight,
		        		RobotAction.Jog,
		        		RobotAction.Jog,
		        		RobotAction.TurnRight,
		        		RobotAction.Jog,
		        		RobotAction.Jog
		        		));
		ArrayList<RobotAction> expectedActions2 = new ArrayList<RobotAction>(
		        Arrays.asList(		RobotAction.Jog,
		        		RobotAction.TurnRight,
		        		RobotAction.Jog));
		
		prepare(data);
		ArrayList<RobotAction> actionsT = new ArrayList<RobotAction>();
		ArrayList<RobotAction> actionsT2 = new ArrayList<RobotAction>();
		m_runningMachine.changeState(RobotRunningState.FindingDroppingArea);
		m_sequence = m_runningMachine.findingDropAreaSequence();
		
		m_mainMachine.start();
		//m_mainMachine.move();
		//actionsT=commander.getActions();
		//assertFalse(m_sequence.succeeded());
		
		//m_mainMachine.move();
		//actionsT=commander.getActions();
		//assertFalse(m_sequence.succeeded());
		
		//m_mainMachine.move();
		//actionsT=commander.getActions();
		//assertFalse(m_sequence.succeeded());
		
		//m_mainMachine.move();
		//actionsT=commander.getActions();
		//assertFalse(m_sequence.succeeded());
		
		//m_mainMachine.move();
		//actionsT=commander.getActions();
		//assertFalse(m_sequence.succeeded());
				
		for(int x=0;x<35;x++){
			m_mainMachine.move();
		}
		//actionsT=commander.getActions();
		assertTrue(m_sequence.succeeded());
		
		actionsT=commander.getActions();
		actionsT2=m_sequence.getHistory();
		assertEquals(expectedActions2,m_sequence.getHistory());
    }
}
